#ifndef MOTOR_CYBERGEAR_H
#define MOTOR_CYBERGEAR_H

#include "fdcan.h"
#include "user_math.h"

typedef enum
{
    HEADER_GET_ID = 0,
    HEADER_CONTROL_MIT = 1,
    HEADER_GET_RESPONSE = 2,
    HEADER_ENABLE = 3,
    HEADER_DISABLE = 4,
    HEADER_SET_ZERO = 6,
    HEADER_SET_ID = 7,
    HEADER_GET_PARAM = 17,
    HEADER_SET_PARAM = 8,
    HEADER_SET_CONTROL_CMD = 18,
    HEADER_GET_ERROR = 21,
} MotorCyberGearHeader;

typedef enum
{
    PARA_RUN_MODE = 0x7005,
    PARA_IQ_REF = 0x7006,
    PARA_SPD_REF = 0x700a,
    PARA_LIMIT_TORQUE = 0x700b,
    //    PARA_CUR_KP = 0x7010,
    //    PARA_CUR_KI = 0x7011,
    //    PARA_CUR_FILT_GAIN = 0x7014,
    PARA_LOC_REF = 0x7016,
    PARA_LIMIT_SPD = 0x7017,
    PARA_LIMIT_CUR = 0x7018,

    PARA_CUR_FILT_GAIN = 0x2011,
    PARA_CUR_KP = 0x2012,
    PARA_CUR_KI = 0x2013,
    PARA_SPD_KP = 0x2014,
    PARA_SPD_KI = 0x2015,
    PARA_LOC_KP = 0x2016,
    PARA_SPD_FILT_GAIN = 0x2017,

} MotorCyberGearParameter;

typedef enum
{
    MODE_MIT = 0,
    MODE_POS = 1,
    MODE_VEL = 2,
    MODE_TORQUE = 3,
} MotorCyberGearMode;

typedef struct
{
    float forward_torque;
    float target_position;
    float target_velocity;
    float kp;
    float ki;
} MotorMIT_Typedef;

typedef struct
{
    uint8_t id;
    uint8_t error_info;
    uint8_t mode_status;
    float actual_position;
    float actual_velocity;
    float actual_torque;
    float actual_temperature;
} MotorStatus_Typedef;

typedef struct
{
    float cur_filt_gain;
    float cur_kp;
    float cur_ki;
    float spd_kp;
    float spd_ki;
    float loc_kp;
    float spd_filt_gain;
} MotorPID_Typedef;

#define id_convert(header, id) ((header << 24) + id)
#define id_convert_mit(header, body, id) ((header << 24) + (body << 8) + id)
#define mit_pos_to_u16(pos) (uint16_t)(2607.555f * (pos + M_4PI_F))
#define mit_vel_to_u16(vel) (uint16_t)(1092.25f * (vel + 30.0f))
#define mit_torque_to_u16(torque) (uint16_t)(2730.625f * (torque + 12.0f))
#define mit_kp_to_u16(kp) (uint16_t)(kp * 131.07f)
#define mit_ki_to_u16(ki) ((uint16_t)(ki) * 13107)

#define u16_to_pos(u16) ((float)(u16 - 0x8000) / 2607.555f)
#define u16_to_vel(u16) ((float)(u16 - 0x8000) / 1092.25f)
#define u16_to_torque(u16) ((float)(u16 - 0x8000) / 2730.625f)
#define u16_to_temperature(u16) ((float)(u16) / 10.0f)

void motor_enable(uint8_t id);
void motor_disable(uint8_t id);
void motor_set_zero(uint8_t id);
void motor_control_mit(uint8_t id, MotorMIT_Typedef *motor_mit);
MotorStatus_Typedef motor_get_status(uint32_t identifier, const uint8_t *data);

void motor_set_mode(uint8_t id, uint8_t mode);
void motor_write_param(uint8_t id, uint16_t index, float data);
void motor_set_control_cmd(uint8_t id, uint16_t index, float data);

void motor_set_current(uint8_t id, float iq);
void motor_set_velocity(uint8_t id, float vel);
void motor_set_position(uint8_t id, float pos);
void motor_set_pid(uint8_t id, MotorPID_Typedef *motor_pid);

#endif // MOTOR_CYBERGEAR_H
